// Standard includes 
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <linux/serial.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <math.h>
#include <iostream>
using std::cout;
using std::endl;

// ROS includes
#include <ros/ros.h>
using ros::NodeHandle;
using ros::Publisher;
using ros::Rate;
using ros::Time;
#include <visualization_msgs/Marker.h>
using visualization_msgs::Marker;
//using geometry_msgs::Point;
#include <hokuyo/hokuyo.h>
using hokuyo::Laser;
using hokuyo::LaserConfig;
using hokuyo::LaserScan;

// Local includes
#include "lidar/dynamixel.h"
#include "path/pathplanner.h"
#include "path/path.h"
#include "path/point.h"

// Control table addresses
#define GOAL_POSITION_L		30
#define MOVING_SPEED_L 		32
#define TORQUE_LIMIT_L		34
#define PRESENT_POSITION_L	36
#define MOVING			46

// Defualt settings
#define DEFAULT_BAUD		1 // MBPS 
#define DEFAULT_ID		1
#define DEFAULT_TORQUE		1023

// Constants
#define ROBOT_WIDTH 100
#define PTS_PER_TEN_CM 6
#define OBSTACLE_GROWTH (ROBOT_WIDTH / 10 / 2) 

// Positions to reach
const int posHigh 		= 445;
const int posLow 		= 555;
const int speedHigh 		= 100; // RPM
const int speedLow 		= 10; // RPM

// 2D array to store 3D data
const int posDiff 		= posLow - posHigh;
const int numScans 		= 722;
const int occHeight 		= 40;
const int occWidth 		= 125;
double map3d[posDiff+1][numScans];
int **occupancy;
int numPoints = 0;

// Function headers
void initializeMaps();
void filterMap(double, int);
void compressMap();
void constructOccupancyGrid();
void expandOccupancyGrid();
void plotRobot(ros::Publisher, double, double, double);
void plot3dMap(ros::Publisher);
void plotOccupancyGrid(ros::Publisher);
void plotPath(Path* path, ros::Publisher marker_pub);

int main(int argc, char** argv) {
	ros::init(argc, argv, "lidar");
	NodeHandle n;
	Publisher markerPub = n.advertise<Marker>("visualization_marker", 10);
	Rate rate(30);

	int index = 0, deviceIndex = 0, moving, presentPos, commStatus;
	int positions[2] = { posHigh, posLow }, speeds[2] = { speedHigh, speedLow };
	Laser lidar = Laser();

	const char *port = (argc < 2 ? "/dev/ttyACM0" : argv[1]);

	// Open/config lidar
	LaserConfig config;
	lidar.open(port);
	lidar.laserOn();
	lidar.getConfig(config);
	cout << config.min_angle << ", " << config.max_angle << ", " << config.max_range << endl;
	config.min_angle = -1.57079633;
	config.max_angle = 1.57079633;

	// Open USB2Dynamixel
	if(dxl_initialize(deviceIndex, DEFAULT_BAUD) == 0) {
		cout << "Failed to open USB2Dynamixel!" << endl;
		return 1;
	}

	// Set torque limit
	dxl_write_word(DEFAULT_ID, TORQUE_LIMIT_L, DEFAULT_TORQUE);
	while(ros::ok()) {
		initializeMaps();

		// Write goal speed/position
		dxl_write_word(DEFAULT_ID, MOVING_SPEED_L, speeds[index]);
		dxl_write_word(DEFAULT_ID, GOAL_POSITION_L, positions[index]);
		do {
			// Read present position
			presentPos = dxl_read_word(DEFAULT_ID, PRESENT_POSITION_L);
			commStatus = dxl_get_result();

			if(commStatus == COMM_RXSUCCESS) {
				printErrorCode();
			} else {
				printCommStatus(commStatus);
				break;
			}

			// Only scan on the way down
			if(index) {
				// Convert raw position to a z-coordinate
				presentPos = posDiff - (presentPos - posHigh);

				LaserScan scan;
				lidar.pollScan(scan, config.min_angle, config.max_angle);
				for(int i = 0; i<(int)scan.ranges.size(); i++) {
					double r = scan.ranges[i];
					double t = (double)i * config.ang_increment;
					double d = r * sin(t);
					d = (d <= 0 ? 0.0 : d);

					map3d[presentPos][i] = r; 			 

					/*
					 * Another linearization method:
					 * d = 2 - 1/r
					 */
				}
			}

			// Check if servo is done moving
			moving = dxl_read_byte(DEFAULT_ID, MOVING);
			commStatus = dxl_get_result();
			if(commStatus == COMM_RXSUCCESS) {
				if(!moving) {
					if(index) {
						//plot3dMap(markerPub);

						// Filter map
						filterMap(0.1, 2);

						// Occupancy grid
						constructOccupancyGrid();
						plotRobot(markerPub, 0, 0, 0);
						plotOccupancyGrid(markerPub);
						expandOccupancyGrid();
	
						// Reverse occ grid
						int** cp = (int**)malloc(40 * sizeof(int *));
						for(int i=0; i<40; i++) {
							cp[i] = (int*) malloc(170 * sizeof(int));
						}

						for(int i=0; i<occHeight; i++) {
							for(int j=0; j<occWidth; j++) {
								cp[i][j] = occupancy[occHeight-1-i][j];
							}
						}

						// Path planning
						Point a(occWidth/2,occHeight-1), b(occWidth/2, 0);
						PathPlanner *pp = new PathPlanner(cp, occWidth, occHeight, a, b);
						pp->astar();
						Path *p = pp->getPath();
						if(p != NULL) {
						 	printPath(p, false);
							plotPath(p, markerPub);
							delete p;
						}
						numPoints = 0;
					}
					index = !index;
				}
			} else {
				break;
			}
		} while(moving);
	}

	// Close devices
	dxl_terminate();
	lidar.laserOff();
	lidar.close();

	return 0;
}

// Initiailize all distance points to 0
void initializeMaps() {
	occupancy = (int**)malloc(occHeight * sizeof(int *));
	for(int i=0; i<occHeight; i++) {
		occupancy[i] = (int*) malloc(occWidth * sizeof(int));
	}

	// Initialize the maps
	for(int i=0; i<numScans; i++) {
		for(int j=0; j<111; j++) {
			map3d[j][i] = 4.0;
		}
	}

	for(int i=0; i<occHeight; i++) {
		for(int j=0; j<occWidth; j++) {
			occupancy[i][j] = 0;
		}
	}
}

void filterMap(double tolerance, int filterSize) {
	// Apply minimum range filter
	for(int i=0; i<111; i++) {
		for(int j=0; j<numScans; j++) {
			double curr = map3d[i][j];
			map3d[i][j] = (curr < 0.2 ? 4.0 : curr);
		}
	}

	// Apply noise filter
	for(int i=0; i<111; i++) {
		for(int j=filterSize; j<numScans-filterSize; j++) {
			double curr = map3d[i][j];
			double currLeft = map3d[i][j-filterSize];
			double currRight = map3d[i][j+filterSize];

			if(curr < 4) {
				if((currLeft > curr-tolerance) && (currRight > curr-tolerance)) {
					map3d[i][j] = currRight;
				} else if((currLeft < curr+tolerance) && (currRight < curr+tolerance)) {
					map3d[i][j] = currRight;
				} else if((currLeft < curr+tolerance) && (currRight > curr-tolerance)) {
					map3d[i][j] = currRight;
				} else if((currLeft > curr-tolerance) && (currRight < curr+tolerance)) {
					map3d[i][j] = currRight;
				}
			}
		}
	}
}

void constructOccupancyGrid() {
	for(int i=0; i<111; i++) {
		int x = 0;
		for(int j=0; j<numScans; j+=PTS_PER_TEN_CM) {
			for(int k=j; k<j+PTS_PER_TEN_CM; k++) {
				double y = map3d[i][k] * 10;
				if(y < 40) {
					occupancy[(int)y][x] = 2;
				}
			}
			if(++x > occWidth-1) {
				break;
			}
		}
	}
}

void expandOccupancyGrid() {
	for(int i=0; i<occHeight; i++) {
		for(int j=0; j<occWidth; j++) {
			if(occupancy[i][j] == 2) {
				int miny = i - OBSTACLE_GROWTH;
				int maxy = i + OBSTACLE_GROWTH;
				int minx = j - OBSTACLE_GROWTH;
				int maxx = j + OBSTACLE_GROWTH;

				while(miny < 0) miny++;
				while(maxy > occHeight-1) maxy--;
				while(minx < 0) minx++;
				while(maxx > occWidth-1) maxx--;

				for(int y=miny; y<maxy; y++) {
					for(int x=minx; x<maxx; x++) {
						occupancy[y][x] = 1;
					}
				}
			} 
		}
	}
}

void plotRobot(ros::Publisher marker_pub, double x, double y, double z) {
	visualization_msgs::Marker robot;
	robot.header.frame_id = "/lidar_frame";
	robot.header.stamp = ros::Time::now();
	robot.ns = "sample";
	robot.action = visualization_msgs::Marker::ADD;

	// Config the robot
	robot.id = numPoints++;
	robot.type = visualization_msgs::Marker::CUBE;
	robot.pose.position.x = x;
	robot.pose.position.y = y;
	robot.pose.position.z = z;
	robot.scale.x = robot.scale.y = robot.scale.z = 0.2;
	robot.color.r = 1.0f;
	robot.color.a = 1.0f; 

	marker_pub.publish(robot);
}

void plot3dMap(ros::Publisher marker_pub) {
	for (int i=0; i<111; i++) {
		visualization_msgs::Marker points;
		points.header.frame_id = "/lidar_frame";
		points.header.stamp = ros::Time::now();
		points.ns = "sample";
		points.action = visualization_msgs::Marker::ADD;
		points.pose.orientation.w = 1.0;

		// Config the points
		points.id = numPoints++;
		points.type = visualization_msgs::Marker::POINTS;
		points.scale.x = 0.03f;
		points.scale.y = 0.03f;
		points.color.g = 1.0f;
		points.color.a = 1.0f;
    
		// Create the vertices for the points
		for(int j=0; j<numScans; j++) {
			double x = (double) j;
			double y = map3d[i][j];
			double z = (double) i;

			if(y > 0.02 && y < 4) {
				geometry_msgs::Point p;
				p.x = x/100 - 3;
				p.y = y;
				p.z = z/50;

				points.points.push_back(p);
			}
		}
		marker_pub.publish(points);
	}
}

void plotOccupancyGrid(ros::Publisher marker_pub) {
	for (int i=0; i<occHeight; i++) {
		visualization_msgs::Marker points;
		points.header.frame_id = "/lidar_frame";

		points.header.stamp = ros::Time::now();
		points.ns = "sample";
		points.action = visualization_msgs::Marker::ADD;
		points.pose.orientation.w = 1.0;

		// Config the points
		points.id = numPoints++;
		points.type = visualization_msgs::Marker::POINTS;
		points.scale.x = 0.03f;
		points.scale.y = 0.03f;
		points.color.g = 1.0f;
		points.color.a = 1.0f;

		// Create the vertices for the points
		for(int j=0; j<occWidth; j++) {
			double x = (double) j - (double)occWidth / 2.0;
			double y = (double) i;
			double z = occupancy[i][j];

			if(z > 0) {
				geometry_msgs::Point p;
				p.x = x / 25;
				p.y = y / 10;
				p.z = z;

				points.points.push_back(p);
			}
		}
		marker_pub.publish(points);

	}
}

void plotPath(Path* path, ros::Publisher marker_pub) {
	while(path != NULL) {
		visualization_msgs::Marker points;
		points.header.frame_id = "/lidar_frame";
		points.header.stamp = ros::Time::now();
		points.ns = "sample";
		points.action = visualization_msgs::Marker::ADD;
		points.pose.orientation.w = 1.0;

		// Config the points
		points.id = numPoints++;
		points.type = visualization_msgs::Marker::POINTS;
		points.scale.x = 0.03f;
		points.scale.y = 0.03f;
		points.color.b = 1.0f;
		points.color.a = 1.0f;

		// Create the vertices for the points
		double x = (double) path->p.x - (double)occWidth / 2.0;
		double y = 39.0 - (double) path->p.y;

		geometry_msgs::Point p;
		p.x = x / 25;
		p.y = y / 10;
		p.z = 2;

		points.points.push_back(p);
		
		marker_pub.publish(points);
		path = path->next;
	}
}

// Print communication result
void printCommStatus(int CommStatus) {
	switch(CommStatus) {
	case COMM_TXFAIL:
		cout << "COMM_TXFAIL: Failed transmit instruction packet!" << endl;
		break;

	case COMM_TXERROR:
		cout << "COMM_TXERROR: Incorrect instruction packet!" << endl;
		break;

	case COMM_RXFAIL:
		cout << "COMM_RXFAIL: Failed get status packet from device!" << endl;
		break;

	case COMM_RXWAITING:
		cout << "COMM_RXWAITING: Now recieving status packet!" << endl;
		break;

	case COMM_RXTIMEOUT:
		cout << "COMM_RXTIMEOUT: There is no status packet!" << endl;
		break;

	case COMM_RXCORRUPT:
		cout << "COMM_RXCORRUPT: Incorrect status packet!" << endl;
		break;

	default:
		cout << "This is unknown error code!" << endl;
		break;
	}
}

// Print error bit of status packet
void printErrorCode() {
	if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1)
		cout << "Input voltage error!" << endl;

	if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1)
		cout << "Angle limit error!" << endl;

	if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1)
		cout << "Overheat error!" << endl;

	if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1)
		cout << "Out of range error!" << endl;

	if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1)
		cout << "Checksum error!" << endl;

	if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1)
		cout << "Overload error!" << endl;

	if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1)
		cout << "Instruction code error!" << endl;
}
